#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include <cmath>
#include <vector>

// 定义一个将度数转换为弧度的函数
double degreesToRadians(double degrees) {
    return degrees * M_PI / 180.0;
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("zc4_moveit_control");

    // 创建MoveGroupInterface对象，指定规划组名称
    moveit::planning_interface::MoveGroupInterface move_group(node, "zc4");

    // 创建PlanningSceneInterface对象
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    // 获取规划组的参考坐标系
    std::string planning_frame = move_group.getPlanningFrame();
    RCLCPP_INFO(node->get_logger(), "Planning frame: %s", planning_frame.c_str());

    // 获取规划组的末端执行器名称
    std::string end_effector_link = move_group.getEndEffectorLink();
    RCLCPP_INFO(node->get_logger(), "End effector link: %s", end_effector_link.c_str());

    // 回到home位置
    move_group.setNamedTarget("middle");
    moveit::planning_interface::MoveGroupInterface::Plan home_plan;
    bool success = (move_group.plan(home_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if (success) {
        move_group.execute(home_plan);
        RCLCPP_INFO(node->get_logger(), "Returned to middle position.");
    } else {
        RCLCPP_ERROR(node->get_logger(), "Failed to return to middle position.");
        rclcpp::shutdown();
        return 0;
    }

    // 设置多个目标点的笛卡尔轨迹
    std::vector<geometry_msgs::msg::Pose> waypoints;

    // 目标点1
    geometry_msgs::msg::Pose pose1;
    pose1.position.x = 0.1038815445000545;
    pose1.position.y = 0.05599530875132761;
    pose1.position.z = 0.287593164792294;
    pose1.orientation.x = -7.443283672575919e-06;
    pose1.orientation.y = 7.57226530269504e-05;
    pose1.orientation.z = 0.12184966603307737;
    pose1.orientation.w = 0.9925485646044253;
    waypoints.push_back(pose1);

    // 目标点2
    geometry_msgs::msg::Pose pose2;
    pose2.position.x = 0.08365577107241515;
    pose2.position.y = 0.1086784487646846;
    pose2.position.z = 0.2876367813618629;
    pose2.orientation.x = 1.760144636797022e-06;
    pose2.orientation.y = 5.322223642645702e-07;
    pose2.orientation.z = 0.24192347154193786;
    pose2.orientation.w = 0.970295333346356;
    waypoints.push_back(pose2);

    // 目标点3
    geometry_msgs::msg::Pose pose3;
    pose3.position.x = -0.035022935822420206;
    pose3.position.y = 0.045609591393929116;
    pose3.position.z = 0.10616480515032783;
    pose3.orientation.x = -0.11166965546205539;
    pose3.orientation.y = 0.447983924420112;
    pose3.orientation.z = 0.21466435979487009;
    pose3.orientation.w = 0.8606738663070932;
    waypoints.push_back(pose3);

    // 目标点4
    geometry_msgs::msg::Pose pose4;
    pose4.position.x = 0.012538573815633489;
    pose4.position.y = 0.07088936910157737;
    pose4.position.z = 0.08703581230320055;
    pose4.orientation.x = -0.08668077802938758;
    pose4.orientation.y = 0.34771541222359786;
    pose4.orientation.z = 0.22590035400245256;
    pose4.orientation.w = 0.9058418542350317;
    waypoints.push_back(pose4);

    // 目标点5
    geometry_msgs::msg::Pose pose5;
    pose5.position.x = 0.04286990171612063;
    pose5.position.y = 0.08700486801760565;
    pose5.position.z = 0.08984505547919774;
    pose5.orientation.x = -0.07676414664118633;
    pose5.orientation.y = 0.30788611594344734;
    pose5.orientation.z = 0.22943420961349906;
    pose5.orientation.w = 0.920148547169824;
    waypoints.push_back(pose5);

    // 目标点6
    geometry_msgs::msg::Pose pose6;
    pose6.position.x = 0.06817749905767405;
    pose6.position.y = 0.10050022127968625;
    pose6.position.z = 0.10951363405984693;
    pose6.orientation.x = -0.050259956089854556;
    pose6.orientation.y = 0.20166294297669674;
    pose6.orientation.z = 0.2367524981177909;
    pose6.orientation.w = 0.9490807388619839;
    waypoints.push_back(pose6);

    // 目标点7
    geometry_msgs::msg::Pose pose7;
    pose7.position.x = 0.07172469621929549;
    pose7.position.y = 0.10240814888996855;
    pose7.position.z = 0.15195414937837406;
    pose7.orientation.x = -1.4721404506932893e-06;
    pose7.orientation.y = 1.3499499148647983e-05;
    pose7.orientation.z = 0.2420830706198754;
    pose7.orientation.w = 0.9702555265160044;
    waypoints.push_back(pose7);

    // 目标点8
    geometry_msgs::msg::Pose pose8;
    pose8.position.x = 0.09079657595784903;
    pose8.position.y = 0.05277330576346592;
    pose8.position.z = 0.1519429764788758;
    pose8.orientation.x = -7.419560621061486e-07;
    pose8.orientation.y = 2.11470330887019e-05;
    pose8.orientation.z = 0.12197170012250899;
    pose8.orientation.w = 0.9925335782337429;
    waypoints.push_back(pose8);

    // 目标点9
    geometry_msgs::msg::Pose pose9;
    pose9.position.x = 0.11075168589927481;
    pose9.position.y = -1.091277667118024e-05;
    pose9.position.z = 0.2875988062936282;
    pose9.orientation.x = 1.8382147442206865e-06;
    pose9.orientation.y = 8.132110012412454e-05;
    pose9.orientation.z = -2.4453694472133078e-05;
    pose9.orientation.w = 0.9999999963927584;
    waypoints.push_back(pose9);

    // 目标点10
    geometry_msgs::msg::Pose pose10;
    pose10.position.x = 0.3035704582748756;
    pose10.position.y = -4.6019088696090045e-05;
    pose10.position.z = 0.30536264212765296;
    pose10.orientation.x = 1.8345570569793093e-06;
    pose10.orientation.y = -7.180863568924419e-05;
    pose10.orientation.z = -6.589662785214818e-05;
    pose10.orientation.w = 0.9999999952488945;
    waypoints.push_back(pose10);    

    // 设置笛卡尔路径规划的目标
    move_group.setStartStateToCurrentState();

    // 设置笛卡尔路径规划的参数
    moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
    double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);

    RCLCPP_INFO(node->get_logger(), "Cartesian path planning %.2f%% achieved", fraction * 100.0);

    // 执行笛卡尔轨迹
    if (fraction == 1.0) {
        move_group.execute(cartesian_plan);
    } else {
        RCLCPP_ERROR(node->get_logger(), "Cartesian path planning failed.");
    }

    rclcpp::shutdown();
    return 0;
}
